/* -*- indent-tabs-mode:T; c-basic-offset:8; tab-width:8; -*- vi: set ts=8:
 * $Id: playback.cpp,v 1.1 2004/09/12 03:40:40 poine Exp $
 *
 * Plays back a state log in real time.
 */

#include <iostream>
#include <cstdio>
#include <cstdlib>
#include <state/Server.h>
#include <read_line.h>

#include <mat/Vector.h>
#include <mat/Vector_Rotate.h>
#include <imu-filter/Compass.h>

using namespace std;
using namespace util;
using namespace libstate;
using namespace libmat;



static int			verbose		= 0;
static int			mode		= 0;
static int			manual		= 0;


static ssize_t
get_data(
	const char *			str,
	const char *			marker,
	double *			x,
	double *			y,
	double *			z
)
{
	char			type[ 128 ];
	size_t			len;
	int			rc;

	rc = sscanf( str, "%s %lf %lf %lf %n",
		type,
		x,
		y,
		z,
		&len
	);

	if( rc == 4 && strncmp( type, marker, 3 ) == 0 )
		return len;

/*
	cerr << "Parse error " << marker << " :"
		<< " rc=" << rc
		<< ": " << str
		<< endl;
*/

	return -1;
}


/*
 * Log lines of the form:
 * 1055701770.283449
 *	gps 9 14 264
 *	vel   0.0500   0.0100  -0.0400
 *	pos  -0.1440  -0.0985   0.0500
 *	raw  -0.0985  -0.0140   0.3084
 */
static void
handle_gps_state(
	Server *		server,
	const char *		str
)
{
	int			len;
	msg_gps_t		msg;
	double			numsv;
	double			sacc;
	double			pacc;

	len = get_data( str, "gps", &numsv, &sacc, &pacc );
	if( len < 0 )
		return;
	str += len;

	msg.numsv	= htons( short(numsv) );
	msg.sacc	= htons( short(sacc) );
	msg.pacc	= htons( short(pacc) );

	len = get_data( str, "vel", &msg.vel_n, &msg.vel_e, &msg.vel_d );
	if( len < 0 )
		return;
	str += len;

	len = get_data( str, "pos", &msg.pos_n, &msg.pos_e, &msg.pos_d );
	if( len < 0 )
		return;
	str += len;

	len = get_data( str, "raw", &msg.raw_pos_n, &msg.raw_pos_e, &msg.raw_pos_d );
	if( len < 0 )
		return;
	str += len;

	if( verbose )
		cout << "Sending GPS state" << endl;

	server->send_packet(
		GPS_STATE,
		(void*) &msg,
		sizeof(msg)
	);
}



static void
handle_ppm(
	Server *		server,
	const char *		str
)
{
	msg_ppm_t		msg;
	char			label[ 128 ];

	sscanf( str, "%s %hd %hd %hd %hd %hd %hd %hd %hd",
		label,
		&msg.roll,
		&msg.pitch,
		&msg.yaw,
		&msg.coll,
		&msg.throttle,
		&msg.manual,
		&msg.mode,
		&msg.extra
	);

	msg.roll	= htons( msg.roll );
	msg.pitch	= htons( msg.pitch );
	msg.yaw		= htons( msg.yaw );
	msg.coll	= htons( msg.coll );
	msg.throttle	= htons( msg.throttle );
	msg.manual	= htons( msg.manual );
	msg.mode	= htons( msg.mode );
	msg.extra	= htons( msg.extra );

	mode		= msg.mode;
	manual		= msg.manual;

	if( verbose )
		cout << "Sending PPM" << endl;

	server->send_packet(
		PPM,
		(void*) &msg,
		sizeof(msg)
	);
}
	

/*
 * Autopilot engagement messages of the form:
 * 1055701964.113708
 *	DESIRED_ANGLE   0.1604   0.0351  -1.3803
 *	012132 011886 012977
 */
static void
handle_desired(
	Server *		server,
	const char *		str
)
{
	int			len;
	msg_desired_angle_t	msg;

	len = get_data( str, "DESIRED_ANGLE", &msg.phi, &msg.theta, &msg.psi );
	if( len < 0 )
		return;
	str += len;

	sscanf( str, "%hd %hd %hd", &msg.roll, &msg.pitch, &msg.yaw );

	msg.roll	= htons( msg.roll );
	msg.pitch	= htons( msg.pitch );
	msg.yaw		= htons( msg.yaw );

	if( verbose )
		cout << "Sending desired" << endl;
	server->send_packet(
		DESIRED_ANGLE,
		(void*) &msg,
		sizeof(msg)
	);
}
		
	
/*
 * Autopilot updates of the form:
 * 1055701964.155606
 *	theta   0.1518   0.0394  -1.3803
 *	ned   4.8402  -7.8082  -4.5247
 *	servo 012196 011809 012978
 */
static void
handle_autopilot(
	Server *		server,
	const char *		str
)
{
	int			len;
	msg_autopilot_t		msg;
	double			roll;
	double			pitch;
	double			yaw;

	len = get_data( str, "theta", &msg.phi, &msg.theta, &msg.psi );
	if( len < 0 )
		return;
	str += len;

	len = get_data( str, "ned", &msg.n, &msg.e, &msg.d );
	if( len < 0 )
		return;
	str += len;

	len = get_data( str, "servo", &roll, &pitch, &yaw );
	if( len < 0 )
		return;
	str += len;

	/* Fake the manual / auto mode for now */
	msg.manual	= htons( manual );
	msg.mode	= htons( mode );
	msg.roll	= htons( short(roll) );
	msg.pitch	= htons( short(pitch) );
	msg.yaw		= htons( short(yaw) );

	if( verbose )
		cout << "Sending autopilot state" << endl;
	server->send_packet(
		AUTOPILOT_STATE,
		(void*) &msg,
		sizeof(msg)
	);
}


/*
 * AHRS + GPS messages of the form:
 * 1055701770.052096
 *	POS   0.0924   0.0025  -0.1346
 *	VEL   0.0400   0.0100  -0.0300
 *	ANG   0.0163   0.0147  -1.5283
 *	PQR   0.0082  -0.0082  -0.0247
 *	MAG  -3.0000 -92.0000 -101.0000
 */
static void
handle_ahrs_state(
	Server *		server,
	const char *		str
)
{
	int			len;
	state_t			state;

	len = get_data( str, "POS", &state.x, &state.y, &state.z );
	if( len < 0 )
		return;
	str += len;

	len = get_data( str, "VEL", &state.vx, &state.vy, &state.vz );
	if( len < 0 )
		return;
	str += len;

	len = get_data( str, "ANG", &state.phi, &state.theta, &state.psi );
	if( len < 0 )
		return;
	str += len;

	len = get_data( str, "PQR", &state.p, &state.q, &state.r );
	if( len < 0 )
		return;
	str += len;

/*
	// We don't measure the rotor moments at all
	state.mx = state.my = state.mz = 0;
*/
	len = get_data( str, "MAG", &state.mx, &state.my, &state.mz );
	if( len < 0 )
		return;
	str += len;


	if( verbose )
		cout << "Sending AHRS state" << endl;
	server->send_packet(
		AHRS_STATE,
		(void*) &state,
		sizeof(state)
	);
}


int
main(
	int			argc,
	char **			argv
)
{
	struct timeval		last = { 0, 0 };
	const int		realtime = 1;
	Server			server( 2002 );

	while( 1 )
	{
		const std::string	line = read_line( cin );
		const char *		str = line.c_str();
		struct timeval		now;
		int			rc;

		if( line == "" )
			break;

		ssize_t len;

		rc = sscanf( str, "%d.%d %n",
			&now.tv_sec,
			&now.tv_usec,
			&len
		);

		if( rc != 2 )
		{
			cerr << "Unable to parse time: " << str << endl;
			continue;
		}

		/* Check for duplicate packets */
		if( last.tv_sec == now.tv_sec
		&&  last.tv_usec == now.tv_usec
		)
			continue;

		str += len;

		if( strncmp( str, "POS", 3 ) == 0 )
			handle_ahrs_state( &server, str );
		else
		if( strncmp( str, "gps", 3 ) == 0 )
			handle_gps_state( &server, str );
		else
		if( strncmp( str, "PPM", 3 ) == 0 )
			handle_ppm( &server, str );
		else
		if( strncmp( str, "theta", 5 ) == 0 )
			handle_autopilot( &server, str );
		else
		if( strncmp( str, "DESIRED_ANGLE", 8 ) == 0 )
			handle_desired( &server, str );
		else
			fprintf( stderr, "Unknown log: '%s'\n", str );

		if( last.tv_sec != now.tv_sec )
		{
			time_t		secs = now.tv_sec;

			printf( "%08d: %s",
				now.tv_sec,
				ctime( &secs )
			);
		}


		if( last.tv_sec == 0 )
		{
			last = now;
			continue;
		}

		timersub( &now, &last, &last );
		const int usec_delay = last.tv_sec * 1000000 + last.tv_usec;
		last = now;

		if( usec_delay < 1000 )
			continue;

		if( server.poll( realtime ? usec_delay - 1000 : 0 ) )
			server.get_packet();

	}

	return 0;
}
